/*
 * Particle.cpp
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#include "Particle.h"

namespace std {

double Particle::sigma2 = 0.9*0.9;

Particle::Particle(double x, double y, double heading,double w): x(x),y(y),p(x,y),heading(heading),w(w) {

}

Particle::~Particle() {

}

void Particle::create_random(vector<Particle>* p, int count, Maze* maze){
	int i = 0;
//	cout << "size: " << p->size() <<endl;
	for( i = 0; i < count; i++){
//		cout << " creating random particle "<<i<<endl;
		Point poin = maze->random_free_place();
//		cout << " Point created random: [" << poin.x << ";" << poin.y << "]" <<endl;
		Particle o(poin);
//		cout << " Particle created random: [" << o.x << ";" << o.y << "] P:["<< o.p.x << ";"<< o.p.y<< "]"<<endl;
		p->push_back(o);
	}
//	cout << i << " c:"<< count << endl;
//	cout << "size: " << p->size() <<endl;
}

void Particle::move_by(double x, double y){
	this->x += x;
	this->y += y;
	this->p = Point(this->x,this->y);
}

double Particle::radians(double d){
	return d * M_PI /180;
}

double Particle::read_sensor(Maze *maze){
	return maze->distance_to_nearest_beacon(p);
}

bool Particle::advance_by(double speed,Maze* m){
	return advance_by(speed,m,true,true);
}
bool Particle::advance_by(double speed, Maze* m, bool noisy){
	return advance_by(speed,m,noisy,false);
}
bool Particle::advance_by(double speed, Maze* m, bool noisy,bool no_checker){
	double h = this->heading;
	if(noisy){
		speed = add_little_noise(speed);
		h = add_little_noise(h);
		h += (((UNIFORM_RAND)*6)-3);
	}
	double r = radians(h);
	this->dx = sin(r)*speed;
	this->dy = cos(r) * speed;
	Point poi (x+dx,y+dy);
//	cout << "["<<poi.x<<";"<<poi.y<<"] "<<m.is_free(poi)<<endl;
	if(!no_checker){
		no_checker = m->is_free(poi);
	}
	if(no_checker){
		this->move_by(dx,dy);
		return true;
	}
	return false;
}

} /* namespace std */
